/*
 * Copyright (c) 2024 Black Sesame Technologies
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "dal_msg/dalidl_runtime_c/message_type_support_struct.h"
#include "dal_connextdds/type_support.hpp"
#include "dal_connextdds/typecode.hpp"
#include "dal_connextdds/dds_api.hpp"

#include "ndds/ndds_c.h"
#include "cdr/cdr_typeObject.h"

#include "rticonnextdds_pubsub.hpp"

#include "mj_interfaces/msg/mjtype.hpp"

struct DAL_Connext_NddsTypePluginI;

DAL_Connext_MessageTypeSupport* type_support = nullptr;
mj_interfaces::msg::Mjtype mjtype;

void test_on_data_available(void* listener_data, DDS_DataReader* reader)
{
    uint32_t loan_len = 0;
    uint32_t loan_next = 0;
    DAL_Connext_UntypedSampleSeq loan_data;
    DDS_SampleInfoSeq loan_info;
    void* ros_message = &mjtype;

    DDS_Boolean is_loan = DDS_BOOLEAN_TRUE;
    DDS_Long data_len = 0;
    void** data_buffer = nullptr;

    DDS_ReturnCode_t rc = DDS_DataReader_read_or_take_instance_untypedI(reader,
                                                                        &is_loan,
                                                                        &data_buffer,
                                                                        &data_len,
                                                                        &loan_info,
                                                                        0,
                                                                        0,
                                                                        DDS_BOOLEAN_TRUE,
                                                                        NULL,
                                                                        1,
                                                                        1,
                                                                        &DDS_HANDLE_NIL,
                                                                        NULL,
                                                                        DDS_ANY_SAMPLE_STATE,
                                                                        DDS_ANY_VIEW_STATE,
                                                                        DDS_ANY_INSTANCE_STATE,
                                                                        DDS_BOOLEAN_TRUE);
    if (DDS_RETCODE_OK != rc) {
        if (DDS_RETCODE_NO_DATA != rc) {
            std::cout << "read_or_take_instance_untypedI fail." << '\n';
            return;
        }
    }
    DAL_Connext_MessagePtrSeq_loan_contiguous(
        &loan_data, reinterpret_cast<DAL_Connext_Message**>(data_buffer), data_len, data_len);

    loan_len = DDS_UntypedSampleSeq_get_length(&loan_data);

    for (; loan_next < loan_len; loan_next++) {
        DAL_Connext_Message* msg = reinterpret_cast<DAL_Connext_Message*>(
            DDS_UntypedSampleSeq_get_reference(&loan_data, static_cast<uint32_t>(loan_next)));
        DDS_SampleInfo* info =
            DDS_SampleInfoSeq_get_reference(&loan_info, static_cast<uint32_t>(loan_next));

        size_t deserialized_size = 0;
        if (info->valid_data) {
            type_support->deserialize(ros_message, &msg->data_buffer, deserialized_size);
            std::cout << "on_data_available: "
                      << "i8: " << (int)mjtype.i8 << ", "
                      << "i16: " << mjtype.i16 << ", "
                      << "str: " << mjtype.str << ", "
                      << "unbound_int_arr.size: " << mjtype.unbound_int_arr.size() << std::endl;
        }
    }
}
void test_on_requested_deadline_missed(void* listener_data,
                                       DDS_DataReader* reader,
                                       const struct DDS_RequestedDeadlineMissedStatus* status)
{
}
void test_on_requested_incompatible_qos(void* listener_data,
                                        DDS_DataReader* reader,
                                        const struct DDS_RequestedIncompatibleQosStatus* status)
{
}
void test_on_sample_rejected(void* listener_data,
                             DDS_DataReader* reader,
                             const struct DDS_SampleRejectedStatus* status)
{
}
void test_on_liveliness_changed(void* listener_data,
                                DDS_DataReader* reader,
                                const struct DDS_LivelinessChangedStatus* status)
{
}
void test_on_sample_lost(void* listener_data,
                         DDS_DataReader* reader,
                         const struct DDS_SampleLostStatus* status)
{
}
void test_on_subscription_matched(void* listener_data,
                                  DDS_DataReader* reader,
                                  const struct DDS_SubscriptionMatchedStatus* status)
{
}

int main(int argc, char** argv)
{
    const dalidl_message_type_support_t& type_supports =
        get_message_type_support_handle<mj_interfaces::msg::Mjtype>();
    type_support = new DAL_Connext_MessageTypeSupport(
        DAL_CONNEXT_MESSAGE_USERDATA, &type_supports, nullptr, nullptr);

    DDS_DomainParticipantFactory* factory = DDS_DomainParticipantFactory_get_instance();
    if (!factory) {
        std::cout << "factory is null" << '\n';
        return 1;
    }

    DDS_DomainParticipantQos* part_qos = new DDS_DomainParticipantQos();
    DDS_DomainParticipantFactory_get_default_participant_qos(factory, part_qos);
    DDS_PropertyQosPolicyHelper_add_property(
        &part_qos->property, "dds.builtin_type.octets.max_size", "2147483647", DDS_BOOLEAN_FALSE);

    DDS_DomainParticipant* participant = DDS_DomainParticipantFactory_create_participant(
        factory, 20, part_qos, NULL, DDS_STATUS_MASK_NONE);
    if (!participant) {
        std::cout << "participant is null" << '\n';
        return 1;
    }

    RegisterType(participant, &type_supports);

    DDS_Publisher* pub = DDS_DomainParticipant_create_publisher(
        participant, &DDS_PUBLISHER_QOS_DEFAULT, NULL, DDS_STATUS_MASK_NONE);
    if (!pub) {
        std::cout << "pub is null" << '\n';
        return 1;
    }

    DDS_Subscriber* sub = DDS_DomainParticipant_create_subscriber(
        participant, &DDS_SUBSCRIBER_QOS_DEFAULT, NULL, DDS_STATUS_MASK_NONE);
    if (!sub) {
        std::cout << "sub is null" << '\n';
        return 1;
    }

    DDS_Topic* topic = DDS_DomainParticipant_create_topic(participant,
                                                          "My_Topic",
                                                          type_support->type_name(),
                                                          &DDS_TOPIC_QOS_DEFAULT,
                                                          NULL,
                                                          DDS_STATUS_MASK_NONE);
    if (!topic) {
        std::cout << "topic is null" << '\n';
        return 1;
    }

    DDS_DataWriterQos* w_qos = new DDS_DataWriterQos();
    DDS_Publisher_get_default_datawriter_qos(pub, w_qos);
    w_qos->publish_mode.kind = DDS_ASYNCHRONOUS_PUBLISH_MODE_QOS;
    DDS_PropertyQosPolicyHelper_add_property(
        &w_qos->property,
        "dds.data_writer.history.memory_manager.fast_pool.pool_buffer_max_size",
        "0",
        DDS_BOOLEAN_FALSE);


    DDS_DataWriter* writer = DDS_Publisher_create_datawriter(
        pub, topic, w_qos, NULL, DDS_STATUS_MASK_NONE);
    if (!writer) {
        std::cout << "writer is null" << '\n';
        return 1;
    }

    DDS_TopicDescription* sub_topic = DDS_Topic_as_topicdescription(topic);

    struct DDS_DataReaderListener reader_listener = DDS_DataReaderListener_INITIALIZER;
    reader_listener.on_data_available = test_on_data_available;
    reader_listener.on_requested_deadline_missed = test_on_requested_deadline_missed;
    reader_listener.on_requested_incompatible_qos = test_on_requested_incompatible_qos;
    reader_listener.on_sample_rejected = test_on_sample_rejected;
    reader_listener.on_sample_lost = test_on_sample_lost;
    reader_listener.on_liveliness_changed = test_on_liveliness_changed;
    reader_listener.on_subscription_matched = test_on_subscription_matched;

    DDS_DataReaderQos* r_qos = new DDS_DataReaderQos();
    DDS_Subscriber_get_default_datareader_qos(sub, r_qos);
    r_qos->reader_resource_limits.dynamically_allocate_fragmented_samples = DDS_BOOLEAN_TRUE;
    DDS_PropertyQosPolicyHelper_add_property(
        &r_qos->property,
        "dds.data_reader.history.memory_manager.fast_pool.pool_buffer_max_size",
        "0",
        DDS_BOOLEAN_FALSE);

    DDS_DataReader* reader = DDS_Subscriber_create_datareader(
        sub, sub_topic, r_qos, &reader_listener, DDS_STATUS_MASK_ALL);
    if (!reader) {
        std::cout << "reader is null" << '\n';
        return 1;
    }

    mj_interfaces::msg::Mjtype mj_type;
    void* ros_message = &mj_type;
    mj_type.str = "mjtype";
    mj_type.unbound_int_arr = std::vector<int32_t>(100, 1);

    while (1) {
        mj_type.i8 += 1;
        mj_type.i16 += 1;

        DAL_Connext_Message user_msg;
        if (DAL_RET_OK != DAL_Connext_Message_initialize(&user_msg, type_support, 0)) {
            std::cout << "DAL_Connext_Message_initialize failed." << '\n';
            return DAL_RET_ERROR;
        }
        user_msg.user_data = ros_message;
        user_msg.serialized = false;

        DDS_DataWriter_write_untypedI(writer, &user_msg, &DDS_HANDLE_NIL);

        std::this_thread::sleep_for(std::chrono::seconds(1));
    }
    return 0;
}